
"""
工具类模块：提供着陆系统所需的各种计算工具
包含：距离计算、方位角计算、垂直速度计算等通用功能
"""
import math
import logging

class LandingUtils:
    """
    着陆工具类：提供各种着陆相关的计算方法
    包括：大圆距离计算、方位角计算、垂直速度计算等
    所有方法都是静态方法，可以直接调用
    """
    def __init__(self):
        self.logger = logging.getLogger(__name__)

    @staticmethod
    def haversine_distance(lat1, lon1, lat2, lon2):
        """计算两点间的大圆距离"""
        R = 6371000  # 地球半径(米)

        phi1 = math.radians(lat1)
        phi2 = math.radians(lat2)
        delta_phi = math.radians(lat2 - lat1)
        delta_lambda = math.radians(lon2 - lon1)

        a = math.sin(delta_phi / 2) ** 2 + \
            math.cos(phi1) * math.cos(phi2) * \
            math.sin(delta_lambda / 2) ** 2
        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))

        return R * c

    @staticmethod
    def calculate_bearing(lat1, lon1, lat2, lon2):
        """计算两点间的方位角"""
        lat1 = math.radians(lat1)
        lon1 = math.radians(lon1)
        lat2 = math.radians(lat2)
        lon2 = math.radians(lon2)

        y = math.sin(lon2 - lon1) * math.cos(lat2)
        x = math.cos(lat1) * math.sin(lat2) - \
            math.sin(lat1) * math.cos(lat2) * math.cos(lon2 - lon1)

        bearing = math.degrees(math.atan2(y, x))
        return (bearing + 360) % 360

    @staticmethod
    def calculate_vertical_speed(alt1, alt2, time_diff):
        """计算垂直速度"""
        return (alt2 - alt1) / time_diff if time_diff > 0 else 0